%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Edited by Shu Jiang, 2011.
% ALL RIGHTS ARE RESERVED.
% mailto: shujiang@tamu.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Non-stance slope
% stance knee
% non-stance knee
% Linearized non-stance slope
% hip position
% Linearized hip position
% use both leg one step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% outputs from humanData
% get average mass
% add path
addpath('/home/shu/Documents/Research_Related/Data/NewFlatGround/')
addpath('~/Documents/Research_Related/COM_HD/')
addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/')
addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/humanData')
addpath('/home/shu/Documents/test/K2_NTorso_HM/data')
addpath('./Func/')
addpath('./PlotFunc/')
% name list
name  = {'Ayushi','Charles','Fred','Martin','Ram','Selina',...
    'lily','po','ryan','vic'};

time_rec = zeros(9,1);
sum_weight = 0;
sum_Lc = 0;
sum_Lt = 0;
sum_LT = 0;
sum_hipVel = 0;
sum_stepLength = 0;

% % initialize
% ave_dataSK_s = size(9,100); % sampling frequence  = 101;

%%

for i = 2:length(name)%:length(name)%[2:4 6:length(name)]
    
% get human data, with breaking pts for each step
humanData = getData(name{1,i});
sum_weight  = sum_weight+humanData.weight; % for average weight
% breaking pts for each step
a = humanData.sns_brk(1);
b = humanData.sns_brk(2);
c = humanData.sns_brk(3);
d = humanData.sns_brk(4);

% position
if i <= 6
    [xpos,ypos,zpos] = getPosCartesian(humanData);
else 
    [xpos,ypos,zpos] = getPosCartesianNew(humanData);
end


% calculate length of each link
% length of calf
Lcl = cal_Lc(xpos.lankle_avg,ypos.lankle_avg,xpos.lknee,ypos.lknee);
Lcr = cal_Lc(xpos.rankle_avg,ypos.rankle_avg,xpos.rknee,ypos.rknee);
Lc = 1/2*(Lcl+Lcr);
sum_Lc = sum_Lc+Lc;

% length of thight;
Ltl = cal_Lt(xpos.lhip,ypos.lhip,xpos.lknee,ypos.lknee);
Ltr = cal_Lt(xpos.rhip,ypos.rhip,xpos.rknee,ypos.rknee);
Lt = 1/2*(Ltl+Ltr);
sum_Lt = sum_Lt+Lt; 

% length of torso
LT = cal_Torso(xpos.hip_avg,ypos.hip_avg,xpos.torso,ypos.torso);
sum_LT = sum_LT+LT;


%%%%%%%%%%%  time  %%%%%%%%%%%%%%
% scale time
% from [c:d]
ns_time_pre = (0:length(b:c)-1)'/480;
ns_ret = ns_time_pre;

xpara_time = 1/ns_ret(end); % simply sacled time
ns_time = ns_ret*xpara_time;


%%%% record time %%%%%%%%
time_rec(i-1) = ns_ret(end);

%%%%%%%%%%%% OUTPUT ANGLE %%%%%%%%%%%%%%%%%
if i==7 || i==8 || i==10
    angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
else
    angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
end

%%%%%%%%%%%% OUTPUT SLOPE %%%%%%%%%%%%%%%%%
if i==7 || i==8 || i==10
    slope = Knee2D_slope(xpos,ypos,zpos,humanData.sns_brk,2);
else
    slope = Knee2D_slope(xpos,ypos,zpos,humanData.sns_brk,1);
end

%%%%%%%%%%%% 1 stance knee  %%%%%%%%%%%%%%%%
[ave_dataSK,ave_timeSK] = BeamData(ns_time,angle.sknee);
ave_dataSK_s(i-1,:) = ave_dataSK;

%%%%%%%%%%%% 2 non-stance knee %%%%%%%%%%%%
[ave_dataNSK,ave_timeNSK] = BeamData(ns_time,angle.nsknee);
ave_dataNSK_s(i-1,:) = ave_dataNSK;


%%%%%%%%%%%%% 3 non-stance slope %%%%%%%%%%%%%%%%%%%%%%%%%
[ave_dataNSL,ave_timeNSL] = BeamData(ns_time,slope.nsslope);
ave_dataNSL_s(i-1,:) =ave_dataNSL; 


%%%%%%%%%%%% 4 hip angle %%%%%%%%%%%%%
[ave_dataHip,ave_timeHip] = BeamData(ns_time,angle.hip);
% plot(ns_ret,ret_torso)
ave_dataHip_s(i-1,:) =ave_dataHip;


%%%%%%%%%%%%% 5 hip position %%%%%%%%%%%%%%%%%
hip_pos_us = xpos.lhip(b:c)'; % unshifted hip position
[ave_dataHP,ave_timeHP] = BeamData(ns_time,hip_pos_us);
ave_dataHP_s(i-1,:) =ave_dataHP;


%%%%%%%%%%%%%% 6  Linearized hip Position %%%%%%%%%%%%
%%%%%%%%%%%%%% -Lc*theta_1 - Lt*(theta_1+theta_2) %%%%%%%%%%
hip_pos_L = -Lc.*angle.sa - Lt.*(angle.sa+angle.sknee);
[ave_dataHPL,ave_timeHPL] = BeamData(ns_time,hip_pos_L);
ave_dataHPL_s(i-1,:) = ave_dataHPL;
% plot(test_output2); hold on;

%%%%%%%%%%%%%%%%%% 7 linearized non stance slope %%%%%%%%%%%%%%
L_NS = (Lc.*(angle.sa+angle.sknee+angle.theta3-angle.theta4-angle.nsknee)+...
    Lt*(angle.sa+angle.sknee+angle.theta3-angle.theta4))/(-Lc-Lt);

[ave_dataLNS,ave_timeLNS] = BeamData(ns_time,L_NS);
ave_dataLNS_s(i-1,:) =ave_dataLNS;

end
% % %%

% average data
ave_weight = sum_weight/(length(name)-1)
ave_Lc = sum_Lc/(length(name)-1) % Length of calf
ave_Lt = sum_Lt/(length(name)-1) % Length of thigh
ave_LT = sum_LT/(length(name)-1)% length of Torso
ave_step_length = sum_stepLength/(length(name)-1)
ave_time = mean(time_rec)

%%
ns_time = mean(time_rec)*ave_timeSK'; 
% 1. stance knee
load('Afit1.mat')
[x_meanSK,upperBSK,lowerBSK]=meanValue(ave_dataSK_s);
meanValuePlotE1(ave_timeSK,upperBSK,lowerBSK,x_meanSK,'fig/SKnee.eps',...
    'Angle(rad)',a0(3,:),2,ns_time,6); 

% 2. non-stance knee
[x_meanNSK,upperBNSK,lowerBNSK]=meanValue(ave_dataNSK_s);
meanValuePlotE1(ave_timeNSK,upperBNSK,lowerBNSK,x_meanNSK,'fig/NSKnee.eps',...
    'Angle(rad)',a0(4,:),2,ns_time,7)


% 3. non stance slope
[x_meanNSL,upperBNSL,lowerBNSL]=meanValue(ave_dataNSL_s);
meanValuePlotE1(ave_timeNSL,upperBNSL,lowerBNSL,x_meanNSL,'fig/NSSlope.eps',...
    'Slope',a0(2,:),2,ns_time,3)


% 7. hip position
[x_meanHP,upperBHP,lowerBHP]=meanValue(ave_dataHP_s);
meanValuePlotE1(ave_timeHP,upperBHP-x_meanHP(1),lowerBHP,x_meanHP-x_meanHP(1),'fig/HipPos.eps',...
    'Position(m)',a0(1,:),1,ns_time,1);


% 4. Linearized non stance slope
load('Afit2.mat')
[x_meanLNS,upperBLNS,lowerBLNS]=meanValue(ave_dataLNS_s);
 meanValuePlotE1(ave_timeLNS,upperBLNS,lowerBLNS,x_meanLNS,'fig/LNS.eps',...
     'Slope',a0(2,:),2,ns_time,4);


% 5. hip angle
load('Afit6.mat')
[x_meanHip,upperBHip,lowerBHip]=meanValue(ave_dataHip_s);
meanValuePlotE1(ave_timeHip,upperBHip,lowerBHip,x_meanHip,'fig/HipAngle.eps',...
    'Angle(rad)',a0(2,:),2,ns_time,5);

% 6 Linearized hip position
[x_meanHPL,upperBHPL,lowerBHPL]=meanValue(ave_dataHPL_s);
meanValuePlotE1(ave_timeHPL,upperBHPL-x_meanHPL(1),lowerBHPL,x_meanHPL-x_meanHPL(1),'fig/HipPosL.eps',...
    'Position(m)',a0(1,:),1,ns_time,2);

%%
ns_time = mean(time_rec)*ave_timeSK'; % averaged time

% 1. hip angle 
hip_angle =x_meanHip';
save('data/mean_data_hipAngle.mat','hip_angle','ns_time')

% 2. hip position 
hip_pos = x_meanHP'-x_meanHP(1);
save('data/mean_data_hippos2.mat','hip_pos','ns_time');

% 3. non-stance knee angle
ns_knee = x_meanNSK';
save('data/mean_data_nsknee.mat','ns_knee','ns_time');

% 4. stance knee angle
s_knee = x_meanSK';
save('data/mean_data_sknee.mat','s_knee','ns_time');

% 5. non-stance slope
ns_slope = x_meanNSL';
save('data/mean_data_nsslope.mat','ns_slope','ns_time');



% 6. linearized non-stance slope
Linear_nsslope = x_meanLNS';
save('data/mean_data_LinearNSslope.mat','Linear_nsslope','ns_time');


% 7. Linearized hip position 
Linear_hippos = x_meanHPL'-x_meanHPL(1);

% fittedmodel1 = fit(ns_time, Linear_hippos, 'poly1');
% a1 = coeffvalues(fittedmodel1)
% 
% Linear_hippos = Linear_hippos-a1(2);
save('data/mean_data_LinearHippos.mat','Linear_hippos','ns_time')

%%
% ave_weight =
% 
%    153
% 
% 
% ave_Lc =
% 
%     0.3806
% 
% 
% ave_Lt =
% 
%     0.4522
% 
% 
% ave_LT =
% 
%     0.3273
% 
% ave_hipVel =
% 
%     1.1142
% ave_time =  0.5718
% %
% ave_step_length =
% 
%     0.6381
% use the velocity 0.8, the average time 0.7976;